#include "ipc/semaphore.h"
#include "core/task.h"
#include "tools/list.h"
#include "cpu/irq.h"

void semaphore_init(semaphore_t *p_sem, int init_count)
{
    p_sem->count = init_count;
    list_init(&p_sem->wait_list);
}

void semaphore_wait(semaphore_t *p_sem)
{
    irq_state_t state = irq_enter_protection();

    if (p_sem->count > 0) {
        p_sem->count--;
    } else {
        task_t *curr = get_current_task();
        task_set_block(curr);
        list_insert_last(&p_sem->wait_list, &curr->wait_node);
        task_dispatch();
    }

    irq_leave_protection(state);
}

void semaphore_notify(semaphore_t *p_sem)
{
    irq_state_t state = irq_enter_protection();

    if (list_count(&p_sem->wait_list) == 0) {
        p_sem->count++;
    } else {
        list_node_t *first_node = list_remove_first(&p_sem->wait_list);
        task_t *task = field_struct_addr(first_node, task_t, wait_node);
        task_set_ready(task);
        task_dispatch();
    }

    irq_leave_protection(state);
}

int semaphore_count(semaphore_t *p_sem)
{
    irq_state_t state = irq_enter_protection();
    int count = p_sem->count;
    irq_leave_protection(state);
    return count;
}